#include "mpu6050.h"
#include "driverPortConfig.h"
#include "Sw_comm_suport.h"

#define MPU6050_ADDRESS	0xD0

/**
 * @brief 
*/
void MPU6050_WaitEvent(I2C_TypeDef* I2Cx, uint32_t I2C_EVENT)
{
	uint32_t timeOut;
	timeOut = 10000;
	while(I2C_CheckEvent(I2Cx, I2C_EVENT) != SUCCESS) {
		timeOut--;
		if(timeOut == 0) {
			break;
		}
	}
}

/**
 * @brief 
*/
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	I2C_GenerateSTART(I2C2, ENABLE);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
	
	I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
	
	I2C_SendData(I2C2, RegAddress);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTING);
	
	I2C_SendData(I2C2, Data);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
	
	I2C_GenerateSTOP(I2C2, ENABLE);
}

/**
 * @brief 
*/
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;
	
	I2C_GenerateSTART(I2C2, ENABLE);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
	
	I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Transmitter);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED);
	
	I2C_SendData(I2C2, RegAddress);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED);
	
	I2C_GenerateSTART(I2C2, ENABLE);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT);
	
	I2C_Send7bitAddress(I2C2, MPU6050_ADDRESS, I2C_Direction_Receiver);
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED);
	
	I2C_AcknowledgeConfig(I2C2, DISABLE);
	I2C_GenerateSTOP(I2C2, ENABLE);
	
	MPU6050_WaitEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED);
	Data = I2C_ReceiveData(I2C2);
	
	I2C_AcknowledgeConfig(I2C2, ENABLE);
	
	return Data;
}

/**
 * @brief 
*/
void MPU6050_Init(void)
{
	RCC_APB1PeriphClockCmd(MPU6050_APB1Periph, ENABLE);
	RCC_APB2PeriphClockCmd(MPU6050_APB2Periph, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure = {
		.GPIO_Mode = GPIO_Mode_AF_OD,
		.GPIO_Pin = MPU6050_SDA_Pin | MPU6050_SCL_Pin,
		.GPIO_Speed = GPIO_Speed_50MHz,
	};
	GPIO_Init(MPU6050_Port, &GPIO_InitStructure);
	
	I2C_InitTypeDef I2C_InitStructure = {
		.I2C_Mode = I2C_Mode_I2C,
		.I2C_ClockSpeed = 50000,
		.I2C_DutyCycle = I2C_DutyCycle_2,
		.I2C_Ack = I2C_Ack_Enable,
		.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
		.I2C_OwnAddress1 = 0x00,
	};
	I2C_Init(I2C2, &I2C_InitStructure);
	I2C_Cmd(I2C2, ENABLE);

	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);
}

/**
 * @brief 
*/
uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

/**
 * @brief 
*/
void SwGetMpu6050Info(mpu6050InfoStru *mpu6050Info)
{
	uint8_t DataH, DataL;								
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);		
	mpu6050Info->accX = (DataH << 8) | DataL;					
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);		
	mpu6050Info->accY = (DataH << 8) | DataL;						
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);		
	mpu6050Info->accZ = (DataH << 8) | DataL;						
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);		
	mpu6050Info->groyX = (DataH << 8) | DataL;						
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);		
	mpu6050Info->groyY = (DataH << 8) | DataL;						
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);		
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);		
	mpu6050Info->groyZ = (DataH << 8) | DataL;						
}
